﻿#include "ConnectPayload.h"
#include "CRC16.h"

int ConnectPayload::Pack(uint8_t* connectPayload) const
{
	if (connectPayload == nullptr) return -1;
	// 设置起始标志
	*((uint16_t*)(connectPayload)) = CONNECT_PAYLOAD_START;
	// 设置数据长度
	*((uint16_t*)(connectPayload + 2)) = (uint16_t)data.size();
	// 设置命令
	*((uint8_t*)(connectPayload + 4)) = cmd;
	// 设置句柄，24位
	connectPayload[5] = (handle & 0x0000FF);
	connectPayload[6] = (handle >> 8) & 0x0000FF;
	connectPayload[7] = (handle >> 16) & 0x0000FF;
	// 设置数据
	if (!data.empty())
		memcpy(connectPayload + 10, data.data(), data.size());
	// CRC16校验，覆盖命令、句柄和数据（第4字节到数据末尾）
	uint16_t crc = CRC16_CCITT(connectPayload + 4, 6 + data.size(), 0xffff);
	*((uint16_t*)(connectPayload + 8)) = crc;
	return (int)(10 + data.size());
}
